classdef ARRK_PP_Controller < handle
    properties (Access = public)
        Lf = 0.0;
        Lr = 0.0;
        El = 0.0;
        Ld = 0.0;
        ExpAng = 0.0;
        Sign = 0.0;
    end
    methods (Access = public)
        function obj = ARRK_PP_Controller(VehLf,VehLr)
            obj.Lf = VehLf;
            obj.Lr = VehLr;
        end
        
        function Cal_Lateral_Dist(obj,VehPr,TrackP)
            x_d = TrackP.x - VehPr.x;
            y_d = TrackP.y - VehPr.y;
            el = -sin(VehPr.yaw) * x_d + cos(VehPr.yaw) * y_d;
            fprintf('VehPr V %f\n',(-el/abs(el))*(VehPr.velocity/abs(VehPr.velocity)));
            obj.Sign = (-el/abs(el));
%             obj.Sign = obj.Sign*(VehPr.velocity/abs(VehPr.velocity));
            obj.El = abs(el);
        end
        
        function Cal_Euclid_Dist(obj,VehPr,TrackP)
            obj.Ld = ((TrackP.x - VehPr.x)^2 + (TrackP.y - VehPr.y)^2)^0.5;
        end
        
        function AngleRate = Control_Process(obj,VehARR,TrackP,Delta_t,mode)
            if mode == 1
                obj.Cal_Lateral_Dist(VehARR.P_r,TrackP);
                obj.Cal_Euclid_Dist(VehARR.P_r,TrackP);
            else
                obj.Cal_Lateral_Dist(VehARR.P_f,TrackP);
                obj.Cal_Euclid_Dist(VehARR.P_f,TrackP);
            end
            exp_steer_wheel_angle = pi - atan(((obj.Ld)^2)/(2 * obj.Lf * obj.El)) - atan(((obj.Ld)^2)/(2 * obj.Lr * obj.El));
            angle_deviation = (obj.Sign * exp_steer_wheel_angle) - VehARR.steer_wheel_angle;
            obj.ExpAng = exp_steer_wheel_angle;
            AngleRate = angle_deviation/Delta_t;
            fprintf('Veh pf (%f ,%f) pr (%f ,%f)\n',VehARR.P_f.x,VehARR.P_f.y,VehARR.P_r.x,VehARR.P_r.y);
            fprintf('ErrL %f %f %f Exp angle %f Curr angle %f\n',obj.El,180*atan(((obj.Ld)^2)/(2 * obj.Lf * obj.El))/pi,180*atan(((obj.Ld)^2)/(2 * obj.Lr * obj.El))/pi, obj.Sign * exp_steer_wheel_angle , VehARR.steer_wheel_angle);
            fprintf('Cal angle deviate %f rate %f\n',angle_deviation,AngleRate);
        end
    end
end